﻿#include "Race.h"
#include "DetourLog.h"
#include <stdio.h>

#include "DetourNavMesh.h"
#include "DetourTileCache.h"
#include "DetourTileCacheBuilder.h"
#include "fixed64Helper.h"
#include "RaceTileCacheLoader.cpp"
#include "Finalizer.h"
#include "InputGeom.h"
#include "DetourCrowd.h"
#include "DetourNavMeshQuery.h"
static fixed64 frand()
{
	return fixed64(FPRaw(rand() & 0xffff));
}

Race::Race()
{
	m_talloc = new LinearAllocator(320000);
	m_tcomp = new FastLZCompressor;
	m_tmproc = new MeshProcess;
	mNavMesh = 0;
	mTileCache = 0;
	mGeom = new InputGeom;
	m_crowd = dtAllocCrowd();

	m_filter = new dtQueryFilter;
	m_filter->setIncludeFlags(SAMPLE_POLYFLAGS_ALL ^ SAMPLE_POLYFLAGS_DISABLED);
	m_filter->setExcludeFlags(0);

	m_filter->setAreaCost(SAMPLE_POLYAREA_GROUND, fixed64::_1);
	m_filter->setAreaCost(SAMPLE_POLYAREA_WATER, fixed64::_10);
	m_filter->setAreaCost(SAMPLE_POLYAREA_ROAD, fixed64::_1);
	m_filter->setAreaCost(SAMPLE_POLYAREA_DOOR, fixed64::_1);
	m_filter->setAreaCost(SAMPLE_POLYAREA_GRASS, fixed64::_2);
	m_filter->setAreaCost(SAMPLE_POLYAREA_JUMP, (fixed64::_1 + fixed64::_0_50));

	m_vod = dtAllocObstacleAvoidanceDebugData();
	m_vod->init(2048);
	m_agentDebug = new dtCrowdAgentDebugInfo;
	m_agentDebug->idx = -1;
	m_agentDebug->vod = m_vod;
}

Race::~Race()
{
	delete m_talloc;
	m_talloc = 0;
	delete m_tcomp;
	m_tcomp = 0;
	delete m_tmproc;
	m_tmproc = 0;
	dtFreeTileCache(mTileCache);
	mTileCache = 0;
	dtFreeNavMesh(mNavMesh);
	mNavMesh = 0;
	if (mGeom)
		delete mGeom;
	mGeom = 0;
	dtFreeCrowd(m_crowd);
	m_crowd = 0;
	if (m_filter)
		delete m_filter;

	dtFreeObstacleAvoidanceDebugData(m_vod);
	delete m_agentDebug;
	m_agentDebug = 0;
}

bool Race::load_navmesh_from_file(const char* pcFileName)
{
	if (!pcFileName)
		return false;
	
	FILE* fp = fopen(pcFileName, "rb");
	if (!fp)
		return false;

	Finalizer tf([&]()->void
		{
			fclose(fp);
		});
	
	fseek(fp, 0, SEEK_END);
	long length = ftell(fp);
	fseek(fp, 0, SEEK_SET);
	if (length <= 0)
		return false;
	
	unsigned char* buf = (unsigned char*)dtAlloc(length, DT_ALLOC_TEMP);
	if (!buf)
	{
		return false;
	}
	Finalizer tb([&]()->void {dtFree(buf); });

	size_t readCnt = fread(buf, length, 1, fp);
	if (readCnt != 1)
	{
		return false;
	}

	return load_navmesh_from_buffer(buf, length);
}

bool Race::load_navmesh_from_buffer(unsigned char* buf, int length)
{
	dtFreeTileCache(mTileCache);
	mTileCache = 0;

	dtFreeNavMesh(mNavMesh);
	mNavMesh = 0;

	if (!buf || length <= 0)
		return false;

	int pos = 0;
	fixedByte endianValue = buf[pos++];

	bool swapEndian = endianValue != getEndianValue();

	// Read header.
	TileCacheSetHeader header;
	if (!read_fixedvalue_for_int(buf, length, pos, header.magic, swapEndian))
		return false;
	if (!read_fixedvalue_for_int(buf, length, pos, header.version, swapEndian))
		return false;
	if (!read_fixedvalue_for_int(buf, length, pos, header.numTiles, swapEndian))
		return false;

	const int TILECACHESET_MAGIC = 'T' << 24 | 'S' << 16 | 'E' << 8 | 'T'; //'TSET';
	if (header.magic != TILECACHESET_MAGIC)
	{
		return false;
	}

	const int TILECACHESET_VERSION = 1;
	if (header.version != TILECACHESET_VERSION)
	{
		return false;
	}

	dtNavMeshParams& dtmp = header.meshParams;
	if (!read_fixed64_v3(buf, length, pos, dtmp.orig,swapEndian))
		return false;
	if (!read_fixed64(buf, length, pos, dtmp.tileWidth, swapEndian))
		return false;
	if (!read_fixed64(buf, length, pos, dtmp.tileHeight, swapEndian))
		return false;
	if (!read_fixedvalue_for_int(buf, length, pos, dtmp.maxTiles, swapEndian))
		return false;
	if (!read_fixedvalue_for_int(buf, length, pos, dtmp.maxPolys, swapEndian))
		return false;

	dtTileCacheParams& dttcp = header.cacheParams;
	if (!read_fixed64_v3(buf, length, pos, dttcp.orig, swapEndian))
		return false;
	if (!read_fixed64(buf, length, pos, dttcp.cs, swapEndian))
		return false;
	if (!read_fixed64(buf, length, pos, dttcp.ch, swapEndian))
		return false;
	if (!read_fixedvalue_for_int(buf, length, pos, dttcp.width, swapEndian))
		return false;
	if (!read_fixedvalue_for_int(buf, length, pos, dttcp.height, swapEndian))
		return false;
	if (!read_fixed64(buf, length, pos, dttcp.walkableHeight, swapEndian))
		return false;
	if (!read_fixed64(buf, length, pos, dttcp.walkableRadius, swapEndian))
		return false;
	if (!read_fixed64(buf, length, pos, dttcp.walkableClimb, swapEndian))
		return false;
	if (!read_fixed64(buf, length, pos, dttcp.maxSimplificationError, swapEndian))
		return false;
	if (!read_fixedvalue_for_int(buf, length, pos, dttcp.maxTiles, swapEndian))
		return false;
	if (!read_fixedvalue_for_int(buf, length, pos, dttcp.maxObstacles, swapEndian))
		return false;

	mNavMesh = dtAllocNavMesh();
	if (!mNavMesh)
	{
		return false;
	}

	dtStatus status = mNavMesh->init(&header.meshParams);
	if (dtStatusFailed(status))
	{
		return false;
	}

	mTileCache = dtAllocTileCache();
	if (!mTileCache)
		return false;

	status = mTileCache->init(&header.cacheParams, m_talloc, m_tcomp, m_tmproc);
	if (dtStatusFailed(status))
	{
		return false;
	}

	for (int i = 0; i < header.numTiles; ++i)
	{
		TileCacheTileHeader tileHeader;
		if (!read_fixedvalue_for_uint(buf, length, pos, tileHeader.tileRef, swapEndian))
			return false;
		if (!read_fixedvalue_for_int(buf, length, pos, tileHeader.dataSize, swapEndian))
			return false;

		if (!tileHeader.tileRef || !tileHeader.dataSize)
			break;

		unsigned char* data = (unsigned char*)dtAlloc(tileHeader.dataSize, DT_ALLOC_PERM);
		if (!data) break;
		memcpy(data, buf + pos, tileHeader.dataSize);
		pos += tileHeader.dataSize;

		dtCompressedTileRef tile = 0;
		dtStatus status = mTileCache->addTile(data, tileHeader.dataSize, DT_COMPRESSEDTILE_FREE_DATA, &tile, swapEndian);
		if (dtStatusFailed(status))
		{
			dtFree(data);
			return false;
		}

		if (tile)
		{
			status = mTileCache->buildNavMeshTile(tile, mNavMesh, swapEndian);
			if (dtStatusFailed(status))
			{
				return false;
			}
		}
	}

	if (pos != length)
		return false;

	return true;
}

bool Race::load_geom(const char* pcFileName)
{
	if (!mGeom)
		return false;

	std::string fileName = pcFileName;
	if (!mGeom->loadMesh(fileName + ".bytes"))
		return false;

	if (!mGeom->loadGeomSet(fileName + "_gset.bytes"))
		return false;

	return true;
}

bool Race::load_mesh(unsigned char* buf, int length)
{
	return mGeom && mGeom->loadMesh(buf, length);
}

bool Race::load_geom_setting(unsigned char* buf, int length)
{
	return mGeom && mGeom->loadGeomSet(buf, length);
}

void Race::update(FPRaw dt)
{
	if (0 != mTileCache && 0 != mNavMesh)
	{
		mTileCache->update(dt, mNavMesh);
	}
	if (mNavMesh!=0 && m_crowd !=0)
	{
		
		m_crowd->update(dt, m_agentDebug);

		// Update agent trails
		//for (int i = 0; i < m_crowd->getAgentCount(); ++i)
		//{
		//	const dtCrowdAgent* ag = crowd->getAgent(i);
		//	AgentTrail* trail = &m_trails[i];
		//	if (!ag->active)
		//		continue;
		//	// Update agent movement trail.
		//	trail->htrail = (trail->htrail + 1) % AGENT_MAX_TRAIL;
		//	dtVcopy(&trail->trail[trail->htrail * 3], ag->npos);
		//}
		m_agentDebug->vod->normalizeSamples();
	}
}

const fixed64* Race::get_mesh_verts(int& vcnt) const
{
	vcnt = 0;
	if (!mGeom)
		return 0;
	const rcMeshLoaderObj* pkMesh = mGeom->getMesh();
	if(!pkMesh)
		return 0;
	vcnt = pkMesh->getVertCount();
	return pkMesh->getVerts();
}

const int* Race::get_mesh_triangles(int& tcnt) const
{
	tcnt = 0;
	if (!mGeom)
		return 0;
	const rcMeshLoaderObj* pkMesh = mGeom->getMesh();
	if (!pkMesh)
		return 0;
	tcnt = pkMesh->getTriCount();
	return pkMesh->getTris();
}

int Race::addAgent(const fixed64* p) {
	dtCrowdAgentParams ap;
	memset(&ap, 0, sizeof(ap));
	if (!mGeom || !m_crowd)
	{
		return -1;
	}

	ap.radius = mGeom->getBuildSettings()->agentRadius;
	ap.height = mGeom->getBuildSettings()->agentHeight;
	ap.maxAcceleration = 8;
	ap.maxSpeed = fixed64::_3 + fixed64::_0_50;
	ap.collisionQueryRange = ap.radius * (fixed64::_10 + fixed64::_2);
	ap.pathOptimizationRange = ap.radius * (fixed64::_10 * fixed64::_3);
	ap.updateFlags = 0;
	ap.updateFlags |= DT_CROWD_ANTICIPATE_TURNS;
	ap.updateFlags |= DT_CROWD_OPTIMIZE_VIS;
	ap.updateFlags |= DT_CROWD_OPTIMIZE_TOPO;
	ap.updateFlags |= DT_CROWD_OBSTACLE_AVOIDANCE;
	ap.obstacleAvoidanceType = (unsigned char)3;
	ap.separationWeight = fixed64::_2;

	return m_crowd->addAgent(p, &ap);
}

void Race::removeAgent(const int idx) {
	if (!m_crowd)
	{
		return;
	}
	m_crowd->removeAgent(idx);
}


void Race::getAgentPos(int idx, fixed64* pos)
{
	pos[0] = 0; pos[1] = 0; pos[2] = 0;
	if (!m_crowd || idx == -1)
	{
		return;
	}
    dtCrowdAgent* ag = m_crowd->getEditableAgent(idx);
	if (!ag) {
		return;
	}
	dtVcopy(pos, ag->npos);
}


bool Race::randomPos(fixed64* pos)
{
	if (!m_crowd)
	{
		return false;
	}
	m_crowd->init(40, fixed64::_0_50 + fixed64::_0_10, mNavMesh);
	//pos[0] = 0; pos[1] = 0; pos[2] = 0;
	//dtPolyRef randomRef = 0;
	//dtStatus status = m_crowd->getNavMeshQuery()->findRandomPoint(m_filter, frand, &randomRef, pos);
	//if (dtStatusSucceed(status))
	//{
	//	return true;
	//}
	//return false;
	return true;
}

bool Race::initCrowd(const int maxAgents, const fixed64 maxAgentRadius)
{
	if (!m_crowd || !mNavMesh)
	{
		return false;
	}
	return m_crowd->init(maxAgents, maxAgentRadius, mNavMesh);
}

bool Race::agentMovePos(int idx, const fixed64* pos)
{
	if (!m_crowd || idx == -1)
	{
		return false;
	}
	const dtNavMeshQuery* navquery = m_crowd->getNavMeshQuery();
	const fixed64* halfExtents = m_crowd->getQueryExtents();
	fixed64 nearest[3];
	dtPolyRef ref = 0;
	dtVcopy(nearest, pos);
	navquery->findNearestPoly(pos, halfExtents, m_filter, &ref, nearest);
	const dtCrowdAgent* ag = m_crowd->getAgent(idx);
	if (ag && ag->active)
	{
		return m_crowd->requestMoveTarget(idx, ref, nearest);
	}
	return false;
}

bool Race::agentMoveVelocity(int idx, const fixed64* vel)
{
	if (!m_crowd || idx == -1)
	{
		return false;
	}
	const dtNavMeshQuery* navquery = m_crowd->getNavMeshQuery();
	const fixed64* halfExtents = m_crowd->getQueryExtents();
	const dtCrowdAgent* ag = m_crowd->getAgent(idx);
	if (ag && ag->active)
	{
		return m_crowd->requestMoveVelocity(idx, vel);
	}
	return false;
}


bool Race::addTempObstacle(fixed64 radius, fixed64 height, const fixed64* pos, int* ref) {
	if (!mTileCache)
		return false;
	fixed64 p[3];
	dtVcopy(p, pos);
	p[1] -= fixed64::_0_50;
	dtStatus status = mTileCache->addObstacle(p, radius, height, (dtObstacleRef*)ref);
	if (dtStatusSucceed(status))
	{
		return true;
	}
	return false;

}

bool Race::removeTempObstacle(int ref)
{
	dtStatus status = mTileCache->removeObstacle((const dtObstacleRef)ref);
	if (dtStatusSucceed(status))
	{
		return true;
	}
	return false;

}

bool Race::stopAgentMove(int idx)
{
	if (!m_crowd || idx == -1)
	{
		return false;
	}
	dtCrowdAgent* ag = m_crowd->getEditableAgent(idx);
	if (ag && ag->active)
	{
		ag->boundary.reset();
		ag->partial = false;

		ag->topologyOptTime = 0;
		ag->targetReplanTime = 0;
		ag->nneis = 0;

		dtVset(ag->dvel, 0, 0, 0);
		dtVset(ag->nvel, 0, 0, 0);
		dtVset(ag->vel, 0, 0, 0);

		ag->desiredSpeed = 0;
		ag->targetState = DT_CROWDAGENT_TARGET_NONE;
		return true;
	}
	return false;
}

bool Race::setAgentPos(int idx, fixed64* pos)
{
	if (!m_crowd || idx == -1)
	{
		return false;
	}
	dtCrowdAgent* ag = m_crowd->getEditableAgent(idx);
	if (ag && ag->active)
	{
		ag->boundary.reset();
		ag->partial = false;

		ag->topologyOptTime = 0;
		ag->targetReplanTime = 0;
		ag->nneis = 0;

		dtVset(ag->dvel, 0, 0, 0);
		dtVset(ag->nvel, 0, 0, 0);
		dtVset(ag->vel, 0, 0, 0);
		ag->desiredSpeed = 0;
		ag->targetState = DT_CROWDAGENT_TARGET_NONE;
		dtVcopy(ag->npos, pos);
		return true;
	}
	return false;
}

